function [total_torque, final_velocity, final_Torque] =...
    Controller_SolarDetector(initial_position, final_position, initial_velocity,...
    initial_Torque, cycleTime)
%Controller_SolarDetector
%   Detailed explanation goes here

%% Motor Characteristics
J = 1.3*10^-5; %moment of inertia of the rotor,  kg*m^2
b = 1.5077E-5; %motor viscous friction constant, Nm*s
Kb = 0.1719;   %emf constant
Kt = 0.94875;  %motor torque constant
R = 1.1;       %armature resistance, ohm
L = 0.0011;    %armature inductance, H
gearRatio = 10000; %N_panel:N_motor
Max_Voltage = 80;  %Voltage input, V

%% Definition of Initial & Final Conditions
t0 = 0;
tfinal = cycleTime;
t = [t0 tfinal];

theta_i = initial_position*2*pi/360;   %deg to rad
theta_f = final_position*2*pi/360;
dtheta_i = initial_velocity*2*pi/60;  %rpm to rad/sec
current_i = initial_Torque/Kt; %Amps

ref = 0.*t + theta_f;
x0 = [theta_i; dtheta_i; current_i];

%% Definition of Motor to Panel System
A = [0 1/gearRatio 0;
    0 -b/J Kt/J
    0 -Kb/L -R/L];
B = [0 ; 0 ; 1/L];
C_posdeg = [360/(2*pi) 0 0];
C_spddeg = [0 (60)/(2*pi*gearRatio) 0];
C_torque = [0 0 Kt*gearRatio];
D = 0;

motor_sys = ss(A,B,C_torque,D);
% [motor_num, motor_den] = ss2tf(A,B,C_torque,D)

%% Design and Simulation of Full-State Feedback Controller
p1 = -2.0+1.0i;
p2 = -2.0-1.0i;
p3 = -1;
Kc = place(A,B,[p1, p2, p3]);
kp = 7.59e4; %Proportional Input Gain to correct system gain of FullState Feedback


opt = stepDataOptions('InputOffset',initial_position/kp,'StepAmplitude',(final_position-initial_position)/kp);
tracker_sys = ss(A-B*Kc,B,C_posdeg,D);
% [tracker_num, tracker_den] = ss2tf(A-B*Kc,B,C_torque,D)
[Y_pos] = step(tracker_sys, tfinal, opt);


% figure
tracker_Tor = ss(A-B*Kc,B,C_torque,D);
[Y_torque, T2] = step(tracker_Tor, tfinal, opt);

% figure
tracker_spd = ss(A-B*Kc,B,C_spddeg,D);
[Y_speed] = step(tracker_spd, tfinal, opt);


%% Calculation of Function Returns
    total_torque = 0;
for i = 1:length(Y_torque)
    total_torque = total_torque + Y_torque(i);
end
total_torque = total_torque*(T2(2)- T2(1));

%final_position = Y_pos(end);
final_velocity = Y_torque(end);
final_Torque = Y_speed(end);

end

